"""
主程序 - 编队模式语音控制
"""
import time
import threading
import queue
from voice_controller import VoiceController
from llm_client import LLMClient
from network_manager import NetworkChecker
from djitellopy import TelloSwarm

class TelloVoiceControl:
    def __init__(self):
        self.llm_client = LLMClient()
        self.voice_controller = VoiceController(self.llm_client)
        self.network_checker = NetworkChecker()
        
        # 编队模式设置 - 只控制一架无人机
        self.swarm = None
        self.single_tello = None
        self.tello_ip = "192.168.37.180"  # 根据您的实际IP调整
        
        self.running = False
        self.connected = False
        self.flying = False
        
        # 心跳机制相关属性
        self.heartbeat_thread = None
        self.heartbeat_running = False
        self.heartbeat_lock = threading.Lock()
        self.command_executing = False  # 标记是否正在执行命令
        self.heartbeat_interval = 2.0  # 心跳间隔（秒）
        
        # 指令队列相关属性
        self.command_queue = queue.Queue()
        self.queue_processor_thread = None
        self.queue_processing = False
    
    def start_heartbeat(self):
        """启动心跳机制"""
        if self.heartbeat_running:
            return
            
        self.heartbeat_running = True
        self.heartbeat_thread = threading.Thread(target=self._heartbeat_worker, daemon=True)
        self.heartbeat_thread.start()
        print("✓ 心跳机制已启动（每2秒发送悬停指令）")
    
    def stop_heartbeat(self):
        """停止心跳机制"""
        self.heartbeat_running = False
        if self.heartbeat_thread:
            self.heartbeat_thread.join(timeout=3)
        print("✓ 心跳机制已停止")
    
    def _heartbeat_worker(self):
        """心跳工作线程"""
        while self.heartbeat_running and self.flying and self.connected:
            try:
                with self.heartbeat_lock:
                    # 只有在不执行命令时才发送心跳
                    if not self.command_executing and self.flying:
                        self.single_tello.send_rc_control(0, 0, 0, 0)
                
                time.sleep(self.heartbeat_interval)
                
            except Exception as e:
                print(f"❌ 心跳发送失败: {e}")
                time.sleep(1)  # 出错时短暂等待后继续
        
        print("💓 心跳线程已退出")
    
    def start_command_queue_processor(self):
        """启动指令队列处理器"""
        if self.queue_processing:
            return
            
        self.queue_processing = True
        self.queue_processor_thread = threading.Thread(target=self._process_command_queue, daemon=True)
        self.queue_processor_thread.start()
        print("✓ 指令队列处理器已启动")
    
    def stop_command_queue_processor(self):
        """停止指令队列处理器"""
        self.queue_processing = False
        if self.queue_processor_thread:
            self.queue_processor_thread.join(timeout=3)
        print("✓ 指令队列处理器已停止")
    
    def _process_command_queue(self):
        """处理指令队列的工作线程"""
        while self.queue_processing and self.running:
            try:
                # 从队列获取指令（阻塞式，超时1秒）
                command = self.command_queue.get(timeout=1)
                
                if command:
                    print(f"📤 从队列执行指令: {command}")
                    success = self._execute_single_command(command)
                    
                    if success:
                        print(f"✅ 指令执行成功: {command}")
                    else:
                        print(f"❌ 指令执行失败: {command}")
                        # 如果是关键指令失败，可以选择清空队列
                        if command in ["takeoff", "land", "stop"]:
                            print("⚠ 关键指令失败，清空剩余队列")
                            self._clear_command_queue()
                    
                    # 指令间延迟，确保无人机稳定
                    time.sleep(0.5)
                    
            except queue.Empty:
                continue
            except Exception as e:
                print(f"❌ 队列处理错误: {e}")
                time.sleep(1)
        
        print("📥 指令队列处理器已退出")
    
    def _clear_command_queue(self):
        """清空指令队列"""
        cleared_count = 0
        while not self.command_queue.empty():
            try:
                self.command_queue.get_nowait()
                cleared_count += 1
            except queue.Empty:
                break
        
        if cleared_count > 0:
            print(f"🗑 已清空 {cleared_count} 条待执行指令")
    
    def add_commands_to_queue(self, commands):
        """将指令列表添加到队列"""
        if isinstance(commands, str):
            commands = [commands]
        
        added_count = 0
        for command in commands:
            if command and command != "unknown":
                self.command_queue.put(command)
                added_count += 1
                print(f"📥 指令已加入队列: {command}")
        
        if added_count > 0:
            print(f"📋 共添加 {added_count} 条指令到队列")
            return True
        else:
            print("❌ 没有有效指令添加到队列")
            return False
    
    def get_queue_status(self):
        """获取队列状态"""
        return self.command_queue.qsize()
    
    def initialize(self):
        """初始化编队语音控制系统"""
        print("=== Tello编队语音控制系统初始化 ===")
        print("注意：使用编队模式，保持互联网连接")
        
        # 1. 确认网络连接（保持互联网连接用于API调用）
        print("1. 检查网络连接...")
        if self.network_checker.check_internet_connection():
            print("✓ 互联网连接正常，可以使用API服务")
        else:
            print("✗ 互联网连接异常，将影响语音识别和LLM功能")
            choice = input("是否继续？(y/n): ")
            if choice.lower() != 'y':
                return False
        
        current_wifi = self.network_checker.get_current_wifi()
        print(f"当前WiFi: {current_wifi}")
        
        # 2. 测试LLM连接
        print("2. 测试LLM API连接...")
        if self.llm_client.test_connection():
            print("✓ LLM API连接正常")
        else:
            print("✗ LLM API连接失败，将使用离线模式")
        
        # 3. 测试语音识别
        print("3. 测试百度语音识别...")
        if self.voice_controller.test_voice_recognition():
            print("✓ 百度语音识别正常")
        else:
            print("✗ 百度语音识别失败，请检查麦克风和网络")
        
        # 4. 编队模式连接Tello
        print("4. 连接Tello（编队模式）...")
        print(f"尝试连接到Tello IP: {self.tello_ip}")
        
        try:
            # 使用编队模式连接单机
            self.swarm = TelloSwarm.fromIps([self.tello_ip])
            self.swarm.connect()
            
            # 获取单机对象
            self.single_tello = self.swarm.tellos[0]
            
            # 检查电池
            battery = self.single_tello.get_battery()
            print(f"✓ Tello连接成功（编队模式）！电池电量: {battery}%")
            
            if battery < 20:
                print("⚠ 电池电量过低，请充电后再飞行")
                choice = input("是否继续？(y/n): ")
                if choice.lower() != 'y':
                    return False
            
            self.connected = True
            
        except Exception as e:
            print(f"✗ Tello连接失败: {e}")
            print("请检查：")
            print("- Tello是否已开启编队模式")
            print("- IP地址是否正确")
            print("- 是否在同一网络下")
            return False
        
        print("=== 编队语音控制系统初始化完成 ===\n")
        return True
    
    def execute_command(self, commands):
        """执行语音命令（支持复合指令）"""
        if not self.connected:
            print("Tello未连接")
            return False
        
        # 如果是单个指令，转换为列表
        if isinstance(commands, str):
            commands = [commands]
        
        print(f"🎯 收到指令序列，共 {len(commands)} 条")
        
        # 检查是否包含紧急停止指令
        if "stop" in commands:
            print("🚨 检测到紧急停止指令，立即执行")
            return self._execute_single_command("stop")
        
        # 如果只有一条指令且是起飞/降落，立即执行
        if len(commands) == 1 and commands[0] in ["takeoff", "land"]:
            return self._execute_single_command(commands[0])
        
        # 多条指令添加到队列
        return self.add_commands_to_queue(commands)
    
    def _execute_single_command(self, command):
        """执行单条指令"""
        if not self.connected:
            print("Tello未连接")
            return False
        
        try:
            # 标记开始执行命令，暂停心跳
            with self.heartbeat_lock:
                self.command_executing = True
            
            # 先检查电池状态
            battery = self.single_tello.get_battery()
            print(f"当前电池: {battery}%")
            
            command_parts = command.split()
            cmd = command_parts[0].lower()
            
            print(f"🚁 执行指令: {command}")
            
            if cmd == "takeoff":
                if not self.flying:
                    try:
                        self.single_tello.takeoff()
                        print("等待起飞完成...")
                        time.sleep(3)  # 等待起飞完成
                        
                        self.flying = True
                        
                        # 起飞成功后启动心跳机制和队列处理器
                        self.start_heartbeat()
                        self.start_command_queue_processor()
                        
                        print("✓ 无人机起飞成功，系统已就绪")
                        return True
                    except Exception as e:
                        print(f"❌ 起飞失败: {e}")
                        return False
                else:
                    print("⚠ 无人机已在飞行中")
                    return False
                    
            elif cmd == "land":
                if self.flying:
                    try:
                        # 清空待执行队列
                        self._clear_command_queue()
                        
                        # 停止心跳机制和队列处理器
                        self.stop_heartbeat()
                        self.stop_command_queue_processor()
                        
                        self.single_tello.land()
                        time.sleep(3)  # 等待降落完成
                        self.flying = False
                        print("✓ 无人机降落成功")
                        return True
                    except Exception as e:
                        print(f"❌ 降落失败: {e}")
                        # 如果降落失败，尝试紧急停止
                        try:
                            print("尝试紧急停止...")
                            self.single_tello.emergency()
                            time.sleep(2)
                            self.flying = False
                            self.stop_heartbeat()
                            self.stop_command_queue_processor()
                            print("✓ 紧急停止成功")
                            return True
                        except:
                            print("❌ 紧急停止也失败")
                            return False
                else:
                    print("⚠ 无人机未在飞行中")
                    return False
                    
            elif cmd == "stop":
                try:
                    # 清空待执行队列
                    self._clear_command_queue()
                    
                    # 停止所有系统
                    self.stop_heartbeat()
                    self.stop_command_queue_processor()
                    
                    self.single_tello.emergency()
                    self.flying = False
                    print("✓ 紧急停止执行")
                    return True
                except Exception as e:
                    print(f"❌ 紧急停止失败: {e}")
                    return False
                        
            elif self.flying:  # 只有在飞行中才执行移动命令
                try:
                    result = False
                    if cmd == "up" and len(command_parts) == 2:
                        distance = max(20, min(500, int(command_parts[1])))
                        print(f"🚁 向上移动 {distance}cm")
                        self.single_tello.move_up(distance)
                        result = True
                        
                    elif cmd == "down" and len(command_parts) == 2:
                        distance = max(20, min(500, int(command_parts[1])))
                        print(f"🚁 向下移动 {distance}cm")
                        self.single_tello.move_down(distance)
                        result = True
                        
                    elif cmd == "left" and len(command_parts) == 2:
                        distance = max(20, min(500, int(command_parts[1])))
                        print(f"🚁 向左移动 {distance}cm")
                        self.single_tello.move_left(distance)
                        result = True
                        
                    elif cmd == "right" and len(command_parts) == 2:
                        distance = max(20, min(500, int(command_parts[1])))
                        print(f"🚁 向右移动 {distance}cm")
                        self.single_tello.move_right(distance)
                        result = True
                        
                    elif cmd == "forward" and len(command_parts) == 2:
                        distance = max(20, min(500, int(command_parts[1])))
                        print(f"🚁 向前移动 {distance}cm")
                        self.single_tello.move_forward(distance)
                        result = True
                        
                    elif cmd == "back" and len(command_parts) == 2:
                        distance = max(20, min(500, int(command_parts[1])))
                        print(f"🚁 向后移动 {distance}cm")
                        self.single_tello.move_back(distance)
                        result = True
                        
                    elif cmd == "rotate_cw" and len(command_parts) == 2:
                        angle = max(1, min(360, int(command_parts[1])))
                        print(f"🔄 顺时针旋转 {angle}度")
                        self.single_tello.rotate_clockwise(angle)
                        result = True
                        
                    elif cmd == "rotate_ccw" and len(command_parts) == 2:
                        angle = max(1, min(360, int(command_parts[1])))
                        print(f"🔄 逆时针旋转 {angle}度")
                        self.single_tello.rotate_counter_clockwise(angle)
                        result = True
                        
                    else:
                        print(f"✗ 未知命令: {command}")
                        result = False
                    
                    if result:
                        print(f"✓ 命令执行完成: {command}")
                        time.sleep(1)  # 命令完成后稳定时间
                    
                    return result
                        
                except Exception as e:
                    print(f"❌ 指令执行失败: {e}")
                    return False
            else:
                print("⚠ 无人机未在飞行中，无法执行移动命令")
                return False
                
        except Exception as e:
            print(f"✗ 命令执行错误: {e}")
            return False
        finally:
            # 恢复心跳（命令执行完毕）
            with self.heartbeat_lock:
                self.command_executing = False

    def get_status(self):
        """获取无人机状态"""
        if not self.connected:
            return "未连接"
        
        try:
            battery = self.single_tello.get_battery()
            queue_size = self.get_queue_status()
            status = f"电池: {battery}% | 飞行状态: {'飞行中' if self.flying else '地面'} | 队列: {queue_size}条指令 | 模式: 复合指令控制"
            return status
        except:
            return "状态获取失败"
    
    def run(self):
        """运行编队语音控制程序"""
        try:
            if not self.initialize():
                print("系统初始化失败，退出程序")
                return
            
            self.running = True
            
            print("=== 编队语音控制模式运行中 ===")
            print("语音控制已启动（百度语音识别 + 复合指令支持）")
            print("保持互联网连接，支持API调用")
            print("支持复合指令：如'先向前飞50厘米，再顺时针旋转45度'")
            print("使用手动触发模式：输入 'a' 开始语音识别")
            print("💡 输入 'q' 退出系统, 输入 's' 查看状态")
            print("-" * 40)
            
            # 主控制循环
            try:
                while self.running:
                    user_input = input("请输入指令 (a=语音识别, s=状态, q=退出): ").strip().lower()
                    
                    if user_input == 'a':
                        print("🎤 开始5秒语音识别...")
                        commands = self.voice_controller.record_and_process_voice()
                        if commands:
                            if isinstance(commands, list) and len(commands) > 1:
                                print(f"🎯 收到复合指令，共{len(commands)}条: {commands}")
                            else:
                                print(f"🎯 收到语音指令: {commands}")
                            
                            success = self.execute_command(commands)
                            if success:
                                self.voice_controller.speak("指令已接收")
                            else:
                                self.voice_controller.speak("指令处理失败")
                    
                    elif user_input == 's':
                        status = self.get_status()
                        print(f"📊 当前状态: {status}")
                        queue_size = self.get_queue_status()
                        if queue_size > 0:
                            print(f"📋 队列中还有 {queue_size} 条指令等待执行")
                    
                    elif user_input == 'q':
                        print("🛑 退出系统")
                        self.running = False
                        break
                        
                    else:
                        print("❌ 无效输入，请输入 'a', 's' 或 'q'")
                        
            except KeyboardInterrupt:
                print("\n收到退出信号...")
                self.running = False
                
        except Exception as e:
            print(f"运行时错误: {e}")
        finally:
            self.shutdown()
    
    def shutdown(self):
        """安全关闭编队语音控制系统"""
        print("正在关闭编队语音控制系统...")
        
        self.running = False
        
        # 停止指令队列处理器
        self.stop_command_queue_processor()
        
        # 清空剩余指令
        self._clear_command_queue()
        
        # 停止心跳机制
        self.stop_heartbeat()
        
        # 停止所有RC控制
        try:
            if self.flying and self.connected:
                print("停止所有移动...")
                self.single_tello.send_rc_control(0, 0, 0, 0)
                time.sleep(0.5)
        except:
            pass
        
        # 停止语音监听
        try:
            self.voice_controller.stop_listening()
        except:
            pass
        
        # 无人机安全降落
        try:
            if self.flying and self.connected:
                print("无人机正在降落...")
                self.single_tello.land()
                time.sleep(3)
                self.flying = False
        except:
            pass
        
        # 断开编队连接
        try:
            if self.swarm:
                self.swarm.end()
                print("编队连接已断开")
        except:
            pass
        
        print("编队语音控制系统已安全关闭")


def main():
    """主函数"""
    print("Tello编队语音控制系统 v2.3")
    print("作者: 杨垚，乔明梁") 
    print("模式: 编队单机控制 + 百度语音识别 + 复合指令支持")
    print("-" * 40)
    
    # 创建并运行编队控制系统
    control_system = TelloVoiceControl()
    control_system.run()

if __name__ == "__main__":
    main()